#include "WPILib.h"
#include "xbox.h"
#include "EncoderTest.h"

class RobotDemo : public SimpleRobot
{

	RobotDrive myRobot; 
	XboxController XboxControls;
	Jaguar Elevator;
	Jaguar ShootTL;
	Jaguar ShootTR;
	Jaguar ShootBL;
	Jaguar ShootBR;
	DigitalInput LimitUp;
	DigitalInput LimitDown;

public:
	RobotDemo(void):
		
		myRobot(1, 2, 3, 4),	
		XboxControls(1),
		Elevator (9),
		ShootTL (5),
		ShootTR (6),
		ShootBL (7),
		ShootBR (8),
		LimitUp (3),
		LimitDown (2)
		
	
	{
		myRobot.SetExpiration(0.1);
		myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);
		myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);
		myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
		myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);
	}


	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(0.5, 0.0); 	
		Wait(2.0); 				
		myRobot.Drive(0.0, 0.0); 	
	}

	
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			float left = XboxControls.GetAxis(XboxControls.LeftY);
			float right = XboxControls.GetAxis(XboxControls.RightY);
			bool elevUp = XboxControls.GetButton (XboxControls.LeftBumper);
			bool elevDown = XboxControls.GetButton (XboxControls.RightBumper);
			//bool armDown = XboxControls.GetButton (XboxControls.LeftClick);
			//bool armUp = XboxControls.GetButton (XboxControls.RightClick);
			myRobot.TankDrive(left, right); 
			
			/*if (shoot >=  0.5) {
				Shooter.Set (0.5);
			}
			else if (shoot <= -0.5){
				Shooter.Set (0.75);
			}
			else if (shoot == 0.0){
				Shooter.Set (0.25);
			}
			else {
				Shooter.Set (0.0);
			}*/
			//
			if (elevUp && elevDown){
				Elevator.Set(0.0);	
			}
			/*else if (elevUp) {
				Elevator.Set (0.5);
			}*/
			//the switch is opposite of what i have
			else if (LimitDown.Get()){
				Elevator.Set (0.5);
			}
			else if (elevDown) {
				Elevator.Set (-0.5);
			}
			else {
				Elevator.Set (0);
			}
			Wait(0.005);				// wait for a motor update time
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

